/*
 * SERVO.c
 *
 * Created: 22.10.2012 14:33:38
 *  Author: mariuvi
 */ 
#include "definitions.h"
#include "SERVO.h"
#include <avr/io.h>

#define PWM_HZ 50
#define F_CPU 8500000

void SERVO_PWM_set(int us_width);

void SERVO_PWM_init()
{	
	//Fast PWM mode, ICR1 as TOP
	BIT_SET(TCCR1B, WGM13);
	BIT_SET(TCCR1B, WGM12);
	BIT_SET(TCCR1A, WGM11);
	
	//Set clock prescaler to 1/8
	BIT_SET(TCCR1B, CS11);
	
	//Set the TOP value
	ICR1 = F_CPU/(8*PWM_HZ);
	
	//Clear OC1A on compare match
	BIT_SET(TCCR1A, COM1A1);
	
	//Set the pulse width to 1.5 ms (center)
	SERVO_PWM_set(1500);
	
	//Set OC1A as output
	BIT_SET(DDRB, PB5);
}


/*!
 * @function	SERVO_PWM_set
 * @discussion	Sets the pulse width of the PWM signal between 900 us. and 2100 us.
 * @param		us_width		The desired pulse width in microseconds
 * @result		None.
 */	
void SERVO_PWM_set(int us_width)
{
	if(us_width > 2100)
	{
		OCR1A = 2100;
	}
	else if(us_width < 900)
	{
		OCR1A = 900;
	}
	else
	{
		OCR1A = us_width;
	}
}

	
void SERVO_set_position(int pos)
{
	int us_width;
	us_width = pos;
	us_width = 6*us_width;
	us_width = us_width+1500;
	SERVO_PWM_set(us_width);		
}